#include "../Mapping.cpp"

void mappingTest(){
      //Goal setup
    Line gl0(0, PI, 100, coordsToAddr(10,10), coordsToAddr(110,10), coordsToAddr(60,10));
    Line gl1(1, PI/2, 50, coordsToAddr(10,10), coordsToAddr(10, 60), coordsToAddr(10, 35));
    Line gl2(2, PI, 100, coordsToAddr(10,60), coordsToAddr(110,60), coordsToAddr(60,60));
    Line gl3(3, PI/2, 50, coordsToAddr(110,10), coordsToAddr(110, 60), coordsToAddr(110, 35));
    Line goalLines[4] = {gl0, gl1, gl2, gl3};
    LineGroup goalGroup(goalLines, 1);
      //Curr setup
    Line cl0(0, PI, 100, coordsToAddr(10,10), coordsToAddr(110,10), coordsToAddr(60,10));
    Line cl1(1, PI/2, 50, coordsToAddr(10,10), coordsToAddr(10, 60), coordsToAddr(10, 35));
    Line cl2(2, PI, 100, coordsToAddr(10,60), coordsToAddr(110,60), coordsToAddr(60,60));
    Line cl3(3, PI/2, 50, coordsToAddr(110,10), coordsToAddr(110, 60), coordsToAddr(110, 35));
    Line currLines[4] = {cl0, cl1, cl2, cl3};
    LineGroup currGroup(currLines, 1);

    
}
